package pcsr.drivers.epuckDriver.command;

import java.util.LinkedList;
import java.util.NoSuchElementException;
import java.util.concurrent.atomic.AtomicBoolean;

import pcsr.drivers.epuckDriver.EpuckSensorType;



public class EPuckCommand implements IePuckUartProtocol, IePuckCommand {

	private class CommandDispatcherThread extends Thread {
		public AtomicBoolean run = new AtomicBoolean();
		int mySleep;

		public void run() {
			while (run.get()) {
				try {
					sleep(mySleep);
				} catch (InterruptedException e) {
				}
				while(!commandQueue.isEmpty())
				{
					String request = null;
					try {
						request = commandQueue.remove();	
					}
					catch (NoSuchElementException nsee) {
						System.err.println(nsee.getMessage());
					}
					
					if(request != null)
					{
						epuckComm.sendData(request);
					}
				}
			}
		}

		CommandDispatcherThread(int sleepTime) {
			super();
			run.set(true);
			mySleep = sleepTime;
		}
	}

	private class CommandResponseThread extends Thread {
		public AtomicBoolean run = new AtomicBoolean();
		int mySleep;

		public void run() {
			while (run.get()) {
				try {
					sleep(mySleep);
				} catch (InterruptedException e) {
				}
				String cmdResponse = epuckComm.readData();
				HandleCommandResponse(cmdResponse);				
			}
		}

		private void HandleCommandResponse(String cmdResponse) {
			if(cmdResponse.length() <= 2) // First char is a '\r'
			{
				// If it's a simple response without parameters, there's nothing to be done.
				return;
			}
			
			char c = cmdResponse.charAt(1); // First char is a '\r'

			switch (c) {
			case Action.ACCELEROMETER :
				String accelerometerArgs = cmdResponse.substring(3);
				accelerometerArgs = accelerometerArgs.substring(0, accelerometerArgs.length() - 1);
				if(accelerometerArgs != null && accelerometerArgs.length() > 0)
				{
					int[] accelerometerIntValues = normalizeCommandResponseArguments(accelerometerArgs);
					
					NotifySensorUpdate(EpuckSensorType.ACCELEROMETER, accelerometerIntValues);
				}
				break;
			case Action.BODY_LED :
				break;
			case Action.SELECTOR :
				break;
			case Action.SET_MOTOR_SPEED :
				break;
			case Action.GET_MOTOR_SPEED :
				String motorSpeedArgs = cmdResponse.substring(3);
				motorSpeedArgs = motorSpeedArgs.substring(0, motorSpeedArgs.length() - 1);
				if(motorSpeedArgs != null && motorSpeedArgs.length() > 0)
				{
					int[] motorSpeedIntValues = normalizeCommandResponseArguments(motorSpeedArgs);
					
					NotifySensorUpdate(EpuckSensorType.SPEED, motorSpeedIntValues);
				}
				break;
			case Action.FRONT_LED :
				break;
			case Action.IR_RECEIVER :
				break;
			case Action.HELP :
				break;
			case Action.GET_CAMERA :
				String cameraArgs = cmdResponse.substring(3);
				if(cameraArgs != null && cameraArgs.length() > 0)
				{
					/*
					Image img;
					img = Toolkit.getDefaultToolkit().createImage(cameraArgs.getBytes());
					*/
				}
				
				break;
			case Action.SET_CAMERA :
				break;
			case Action.CALIBRATE_SENSORS :
				break;
			case Action.LED :
				break;
			case Action.FLOOR_SENSOR :
				break;
			case Action.PROXIMITY :
				String proximityArgs = cmdResponse.substring(3);
				proximityArgs = proximityArgs.substring(0, proximityArgs.length() - 1);
				if(proximityArgs != null && proximityArgs.length() > 0)
				{
					int[] proximityIntValues = normalizeCommandResponseArguments(proximityArgs);
					
					NotifySensorUpdate(EpuckSensorType.PERIMETER, proximityIntValues);
				}
				break;
			case Action.LIGHT :
				break;
			case Action.SET_MOTOR_POSITION :
				break;
			case Action.GET_MOTOR_POSITION :
				String motorPositionArgs = cmdResponse.substring(3);
				motorPositionArgs = motorPositionArgs.substring(0, motorPositionArgs.length() - 1);
				if(motorPositionArgs != null && motorPositionArgs.length() > 0)
				{
					int[] motorPositionIntValues = normalizeCommandResponseArguments(motorPositionArgs);
					
					NotifySensorUpdate(EpuckSensorType.POSITION, motorPositionIntValues);
				}
				break;
			case Action.RESET :
				break;
			case Action.STOP :
				break;
			case Action.PLAY_SOUND :
				break;
			case Action.MICROPHONE :
				String microphoneArgs = cmdResponse.substring(3);
				microphoneArgs = microphoneArgs.substring(0, microphoneArgs.length() - 1);
				if(microphoneArgs != null && microphoneArgs.length() > 0)
				{
					int[] microphoneIntValues = normalizeCommandResponseArguments(microphoneArgs);
					
					NotifySensorUpdate(EpuckSensorType.MOCROPHONE, microphoneIntValues);
				}
				break;
			case Action.VERSION : 
				break;
			default:
				System.out.println("C='"+c+"' RESPONSE: '" + cmdResponse + "'");
			break;
			}
		}

		private int[] normalizeCommandResponseArguments(String args) {
			String[] values = args.split(",");
			int[] intValues = new int[values.length];
			for(int i = 0; i < values.length; i++)
			{
				try
				{
					intValues[i] = Integer.parseInt(values[i]);
				}
				catch (NumberFormatException nfe) {
					intValues[i] = -1;
				}
			}
			return intValues;
		}

		CommandResponseThread(int sleepTime) {
			super();
			run.set(true);
			mySleep = sleepTime;
		}
	}
	private class SensorStatusResquestThread extends Thread {
		public AtomicBoolean run = new AtomicBoolean();
		int mySleep;

		public void run() {
			CalibrateProximitySensors();
			while (run.get()) {
				try {
					sleep(mySleep);
				} catch (InterruptedException e) {
				}
				getProximitySensors();
				getMicrophoneSensor();
				getAccelerometer();
				//getMotorPosition();
				//getMotorSpeed();
			}
		}

		SensorStatusResquestThread(int sleepTime) {
			super();
			run.set(true);
			mySleep = sleepTime;
		}
	}
	static EPuckCommand singleton = new EPuckCommand();
	private LinkedList<String> commandQueue;
	private LinkedList<ISensorStatusObserver> sensorObservers;
	
	CommandDispatcherThread commandDispatcher = new CommandDispatcherThread(10*100);

	CommandResponseThread commandResponseHandler = new CommandResponseThread(8*10);

	IePuckComm epuckComm;

	SensorStatusResquestThread updateSensors = new SensorStatusResquestThread(20*100);

	public static IePuckCommand getSingleton() {
		return singleton;
	}

	@Override
	public void CalibrateProximitySensors() {
		String request = new String(new char[]{Action.CALIBRATE_SENSORS, COMMAND_TERMINATOR});
		epuckComm.sendData(request);
	}

	/**
	 * Call the close() command when your program ends, otherwise you
	 * will have to turn the NXT brick off/on before you run another
	 * program using iCommand.
	 *
	 */
	public void close() {
		singleton.commandDispatcher.interrupt();
		singleton.Stop();
		singleton.epuckComm.close();
	}

	@Override
	public void getAccelerometer() {
		String request = new String(new char[]{Action.ACCELEROMETER, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void getCamera() {
		//byte[] request = new byte[]{(byte) 0xb7,0x00};
		//((EPuckCommBlueCove)epuckComm).sendData(request);
		((EPuckCommBlueCove)epuckComm).getcameradata();
		
	}

	@Override
	public DeviceInfo getDeviceInfo() {
		// !! Needs to check port to verify they are correct ranges.
		/*
		byte [] request = {PACKETTYPE.REQUEST};
		epuckComm.sendData(request);
		byte [] reply = epuckComm.readData();
		 */
		DeviceInfo d = new DeviceInfo();

		return d;
	}

	@Override
	public String getDeviceName() {
		String ret = null;
		/*
		byte [] request = {STX, PACKETTYPE.REQUEST, ETX, 0x00, 0x00, 0x55, 0x03};
		byte [] reply = new byte[70];
		epuckComm.sendData(request);
		reply = epuckComm.readData();
		DeviceInfo d = new DeviceInfo();
		 */
		return ret;


	}

	@Override
	public void getLightSensors() {
		String request = new String(new char[]{Action.LIGHT, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void getMicrophoneSensor() {
		String request = new String(new char[]{Action.MICROPHONE, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void getMotorPosition() {
		String request = new String(new char[]{Action.GET_MOTOR_POSITION, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void getMotorSpeed() {
		String request = new String(new char[]{Action.GET_MOTOR_SPEED, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void getProximitySensors() {
		String request = new String(new char[]{Action.PROXIMITY, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void getSelector() {
		String request = new String(new char[]{Action.SELECTOR, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void getVersion() {
		String request = new String(new char[]{Action.VERSION, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void NotifySensorUpdate(EpuckSensorType s, int[] args) {
		for(ISensorStatusObserver obs : sensorObservers)
		{
			obs.updateSensor(s, args);
		}
	}

	@Override
	public void observeSensors(ISensorStatusObserver so) {
		sensorObservers.add(so);
	}

	public void open() throws RuntimeException{

		if(singleton.epuckComm != null)
		{
			return;
		}		
		singleton.epuckComm = EPuckCommFactory.createInstance();
		try {
			singleton.epuckComm.open();
		} catch (Exception e) {
			System.err.println("EPuckCommand.open(): Error while connecting ...");
			throw new RuntimeException(e);
		}
		
		singleton.getVersion();
		
		singleton.commandDispatcher.setDaemon(true);
		singleton.commandDispatcher.start();

		singleton.commandResponseHandler.setDaemon(true);
		singleton.commandResponseHandler.start();
		
		singleton.updateSensors.setDaemon(true);
		singleton.updateSensors.start();		
	}

	@Override
	public void PlaySound(Sounds sound) {
		String request = new String(new char[]{Action.PLAY_SOUND,COMMAND_PARAMETER_SEPARATOR , sound.Value(), COMMAND_TERMINATOR});

		commandQueue.addLast(request);	
	}

	@Override
	public void Reset() {
		String request = new String(new char[]{Action.RESET, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void setBodyLed(LedStatus status) {
		if(status == null) return;
		String request = new String(new char[]{Action.BODY_LED,COMMAND_PARAMETER_SEPARATOR , status.Value(), COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}
	
	@Override
	public void setFrontLed(LedStatus status) {
		if(status == null) return;
		String request = new String(new char[]{Action.FRONT_LED,COMMAND_PARAMETER_SEPARATOR , status.Value(), COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}
	
	@Override
	public void setLed(LedNumber led, LedStatus status) {
		String request = new String(new char[]{Action.LED,COMMAND_PARAMETER_SEPARATOR , led.Value(), COMMAND_PARAMETER_SEPARATOR, status.Value(), COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}

	@Override
	public void setMotorPosition(int left, int right) {
		String request = ""+ Action.SET_MOTOR_POSITION + COMMAND_PARAMETER_SEPARATOR + left + COMMAND_PARAMETER_SEPARATOR + right + COMMAND_TERMINATOR;
		commandQueue.addLast(request);
	}

	@Override
	public void setMotorSpeed(int left, int right) {
		String request = ""+ Action.SET_MOTOR_SPEED + COMMAND_PARAMETER_SEPARATOR + left + COMMAND_PARAMETER_SEPARATOR + right + COMMAND_TERMINATOR;
		commandQueue.addLast(request);
	}
	
	@Override
	public void Stop() {
		String request = new String(new char[]{Action.STOP, COMMAND_TERMINATOR});
		commandQueue.addLast(request);
	}
	
	@Override
	public void unobserveSensors(ISensorStatusObserver so) {
		sensorObservers.remove(so);
	}

	// Ensure no one tries to instantiate this.
	private EPuckCommand() 
	{ 
		commandQueue = new LinkedList<String>();
		sensorObservers = new LinkedList<ISensorStatusObserver>();
	}

}
